package org.lovelandhs.robotics.y2010.camera;

import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import edu.wpi.first.wpilibj.image.ColorImage;
import edu.wpi.first.wpilibj.image.NIVisionException;
import org.lovelandhs.robotics.y2010.Robot;

/**
 *
 * @author Robotics
 */
public class Camera implements Runnable {

	public static final double THRESHOLD = 0.01;

	private AxisCamera camera;
	private TrackerDashboard dashboard;
	private Servo pan;
	private Servo tilt;

	private double hAngle = 90;
	private double vAngle = 90;

	private boolean searchLeft;
	private boolean searchUp;

	private boolean paused;

	private int lastSight;

	public Camera() {
		init();
	}

	private void init() {
		paused = true;

		camera = AxisCamera.getInstance();
		camera.writeResolution(AxisCamera.ResolutionT.k320x240);
		camera.writeBrightness(0);

		dashboard = new TrackerDashboard();
		//gyro = new Gyro(1);

		pan = new Servo(3);
		tilt = new Servo(4);
	}

	public void turn() {

		try {
			Gyro gyro = Robot.get().getHardware().getGyro();
			if (camera.freshImage()) {// && turnController.onTarget()) {
				double gyroAngle = gyro.pidGet();
				ColorImage image = camera.getImage();
				Thread.yield();
				Target[] targets = Target.findCircularTargets(image);
				Thread.yield();
				image.free();
				if (targets.length == 0 || targets[0].m_score < THRESHOLD) {
					//System.out.println("No target found");
					lastSight++;
					Target[] newTargets = new Target[targets.length + 1];
					newTargets[0] = new Target();
					newTargets[0].m_majorRadius = 0;
					newTargets[0].m_minorRadius = 0;
					newTargets[0].m_score = 0;
					for (int i = 0; i < targets.length; i++) {
						newTargets[i + 1] = targets[i];
					}
					dashboard.updateVisionDashboard(0.0, gyro.getAngle(), 0.0, 0.0, newTargets);
					if(lastSight >= 10){
						search();
					}
				} else {
					lastSight = 0;
					System.out.println(targets[0]);
					//System.out.println("Target Angle: " + targets[0].getHorizontalAngle());
					//turnController.setSetpoint(gyroAngle + targets[0].getHorizontalAngle());
					dashboard.updateVisionDashboard(0.0, gyro.getAngle(), 0.0, targets[0].m_xPos / targets[0].m_xMax, targets);

					hAngle += targets[0].getHorizontalAngle();
					vAngle -= targets[0].getVerticalAngle();

					//System.out.println("[Debug] Dimensions: " + targets[0].width + "x" + targets[0].height);
					//System.out.println("[Debug] TDistance: " + targets[0].getDistanceTrig(90 - pan.getAngle()));
				}
			}
		} catch (NIVisionException ex) {
			ex.printStackTrace();
		} catch (AxisCameraException ex) {
			ex.printStackTrace();
		}

		pan.setAngle(hAngle);
		tilt.setAngle(vAngle);
		//tilt.set((control.getY() / 2) + 0.5);
	}
	/**
	 *  A fnuction to search for the target.
	 */
	private void search() {
		if(searchLeft) {
			if(hAngle > 0) {
				hAngle -= 10;
			} else {
				searchLeft = false;
				vAngle += ((searchUp)?(10):(-10));
			}
		} else {
			if(hAngle < 90) {
				hAngle += 10;
			} else {
				searchLeft = true;
				vAngle += ((searchUp)?(10):(-10));
			}
		}
		if(vAngle > 130) {
			searchUp = false;
		} else if(vAngle < 70) {
			searchUp = true;
		}
	}

	public void pause() {
		paused = true;
	}

	public void unpause() {
		paused = false;
	}

	public void run() {
		while (true) {
			if (!paused) {
				turn();
			}

			try {
				Thread.sleep(50);
			} catch (Exception ex) {

			}
		}
	}
}
